/******************************************************************************
 * ServoController
 *
 * Description:
 *  This class interfaces with the servo motors via the PWM ports. The PWM
 *  channels were initialized. Other classes can change the duty cycle of
 *  individual motors using the SetPosition function.
 *
 * 
 * Authors:
 *  James O Donoghue (11/6/11)
 *  Russell Martin (11/7/2011)
 *  Maxwell Macri (11/6/11)
 *  Christopher Wood (11/6/11)
 *
 * Contributors:
 *  
 *****************************************************************************/

#include "ServoController.h"

// Determine if motor 0 or 1 is used based on top bit
#define MOTOR1_FLAG   (0x80)
#define POSITION_MASK (0x7F)

// PWM register constants
#define PWMREG_PWMPOL   (0x0A)
#define PWMREG_PWMCTL   (0x30)
#define PWMREG_PWMPER01 (40000)
#define PWMREG_PWMDTY01 (2000)
#define PWMREG_PWMPER23 (40000)
#define PWMREG_PWMDTY23 (2000)
#define PWMREG_PWME     (0x0A)

// Initializes PWM settings
//--------------------------------------------------------------       
void InitalizePWM(void)
{
  PWMPOL = PWMREG_PWMPOL;     // High at beginning, low at end (active high).
  PWMPRCLK = 0x00;            // Prescaler.
  PWMCLK = 0x00;              // Sets clock A to be ch 1 clock.
  PWMCAE = 0x00;              // Sets left aligned PWM.
  PWMCTL = PWMREG_PWMCTL;     // Sets up ch0+1 16bit.
  PWMPER01 = PWMREG_PWMPER01;
  PWMDTY01 = PWMREG_PWMDTY01;
  PWMPER23 = PWMREG_PWMPER23;
  PWMDTY23 = PWMREG_PWMDTY23;
  PWME = PWMREG_PWME;
}

void SetPosition(INT8 value)
{
  // 16 bits for proper scaling into duty-cycle registers
  UINT16 motor;
  UINT16 position;
  
  // First bit determines which motor
  motor=value & MOTOR1_FLAG;
  
  // Other 7 bits determine position
  position=value & POSITION_MASK;
    
  // Now convert the value of the input to the position
  if (motor == 0) // Positive voltage
  {
    // Set positive motor to position
    PWMDTY01 = POS0 + (POSITION_MULT * position);
        
    // Set other motor to 0 position (because 0 negative voltage)
    PWMDTY23 = POS5;
  }
  else // Negative voltage
  {
    // Because position is negative, bit flip from 2's complement
    // to 7 bits unsigned position
    position = ~(position) + 1;
    position = position & POSITION_MASK;
        
    // Set negative motor to position
    PWMDTY23 = POS5 - (POSITION_MULT * position);
        
    // Positive Motor to 0 value (no positive voltage component)
    PWMDTY01 = POS0;
  }
}